#include "jode.h"

#define MAX_CALLBACK_CONTACTS 12

#define MAX_STEP_CONTACTS 4096
#define FLOAT_CONTACT_CHUNKSIZE 20
#define FLOAT_CONTACTBUFFER_SIZE sizeof(float)*MAX_STEP_CONTACTS*FLOAT_CONTACT_CHUNKSIZE

// Create dContactGeom array used in each callback
static dContactGeom * callbackContactGeoms = NULL;
static float *floatContactBuf = NULL;
static dJointGroupID contactGroupID;
static dWorldID worldID;

static int surfaceMode;
static float surfaceMu;
static float surfaceMu2;
static float surfaceBounce;
static float surfaceBounceVel;
static float surfaceMotion1;
static float surfaceMotion2;
static float surfaceSlip1;
static float surfaceSlip2;
static float surfaceSoftErp;
static float surfaceSoftCfm;


void collideGeoms(dContactGeom contactGeom, dGeomID o1, dGeomID o2) {
	dContact contact;
    dJointID c;

    //printf("collide geom o1(0x%x) and o2(0x%x)\n", (unsigned int)o1, (unsigned int) o2);
    
      // Set contact geom
      contact.geom = contactGeom;
      // Set surface parameters
      contact.surface.mode = surfaceMode ;
      contact.surface.mu = surfaceMu;
      contact.surface.mu2 = surfaceMu2;
      contact.surface.bounce = surfaceBounce;
      contact.surface.bounce_vel = surfaceBounceVel;
	  contact.surface.soft_erp = surfaceSoftErp;
      contact.surface.soft_cfm = surfaceSoftCfm;
      contact.surface.motion1 = surfaceMotion1;
      contact.surface.motion2 = surfaceMotion2;
      contact.surface.slip1 = surfaceSlip1;
      contact.surface.slip2 = surfaceSlip2;

      // Create contact joint
	  //printf("create a contact joint (0x%x, 0x%x, 0x%x)\n", (unsigned int) worldID, (unsigned int) contactGroupID, (unsigned int) &contact);
      c = dJointCreateContact (worldID,contactGroupID,&contact);
	  //printf("contact joint created 0x%x\n", (unsigned int)c);
      dJointAttach (c, dGeomGetBody(o1), dGeomGetBody (o2));
}

void nearCallback (void *data, dGeomID o1, dGeomID o2) {
    if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) {
      // colliding a space with something
      dSpaceCollide2 (o1,o2,data,&nearCallback);
      // collide all geoms internal to the space(s)
      if (dGeomIsSpace (o1)) dSpaceCollide ((struct dxSpace *)o1,data,&nearCallback);
      if (dGeomIsSpace (o2)) dSpaceCollide ((struct dxSpace *)o2,data,&nearCallback);
    }
    else {
      // colliding two non-space geoms, so generate contact
      // points between o1 and o2
      int num_contact = dCollide (o1,o2,MAX_CALLBACK_CONTACTS,callbackContactGeoms,sizeof(dContactGeom));
      // add these contact points to the simulation
	  int i;
	  for (i=0; i< num_contact; i++) {
        collideGeoms(callbackContactGeoms[i], o1, o2);
	  }
    }
}

void ODESpaceCollide(dSpaceID space) {
	// empty the joint group
	dJointGroupEmpty(contactGroupID);
	dSpaceCollide(space, NULL, nearCallback);
}

void ODESpaceInit(dWorldID ID) {
	int maxCallbackContactGeoms = MAX_CALLBACK_CONTACTS;
	int maxStepContacts = MAX_STEP_CONTACTS;
	int floatContactBufChunkSize = FLOAT_CONTACT_CHUNKSIZE;

    callbackContactGeoms = (dContactGeom *)malloc(maxCallbackContactGeoms * sizeof(dContactGeom));
    floatContactBuf = (float *) malloc(sizeof(float)*maxStepContacts*floatContactBufChunkSize);

	worldID = ID;
	contactGroupID = dJointGroupCreate(0);


	surfaceMode = dContactMu2 | dContactFDir1 | dContactBounce;
	surfaceMu = 5;
	surfaceMu2 = 5;
	surfaceBounce = (float)0.2;
	surfaceBounceVel = 1;
}